/*
 * @Description: 地图匹配定位算法
 * @Author: Zhijian Qiao
 * @Date: 2020-02-04 18:52:45
 */
#ifndef avp_slam_MATCHING_MATCHING_HPP_
#define avp_slam_MATCHING_MATCHING_HPP_

#include <deque>
#include <Eigen/Dense>
#include <yaml-cpp/yaml.h>

#include "sensor_data/cloud_data.hpp"
#include "sensor_data/pose_data.hpp"
#include "models/registration/registration_interface.hpp"
#include "models/cloud_filter/cloud_filter_interface.hpp"
#include "models/cloud_filter/box_filter.hpp"
#include "models/scan_context/Scancontext.h"
#include<Eigen/StdVector>
#include "tools/tic_toc.hpp"

namespace avp_slam {
    class Matching {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

        Matching();

        void Update(const CloudData &cloud_data, const Eigen::Matrix4f &predict_other = Eigen::Matrix4f::Identity());

        bool SetGNSSPose(const CloudData &cloud_data, const Eigen::Matrix4f &init_pose);

        bool AnalyseInitPose();

        bool
        TryInitPoseSimply(const CloudData &cloud_data, const Eigen::Matrix4f &initPose = Eigen::Matrix4f::Identity());

        void GetGlobalMap(CloudData::CLOUD_PTR &global_map);

        CloudData::CLOUD_PTR &GetLocalMap();

        CloudData::CLOUD_PTR &GetCurrentScan();

        bool HasInited();

        bool HasNewGlobalMap();

        bool HasNewLocalMap();

        bool InitScanContext(const YAML::Node &config_node);

        bool InitPose(const YAML::Node &config_node);

        bool Initialize(const Eigen::Matrix4f &init_pose, const CloudData &cloud_data);

        inline Eigen::Matrix4f GetInitPose() { return init_pose_; }

        inline Eigen::Matrix4f GetResultPose() { return result_pose_; }

    private:

        bool ReadConfig();

        bool InitRegistration(std::shared_ptr<RegistrationInterface> &registration_ptr, const YAML::Node &config_node);

        bool InitFilter(std::string filter_user, std::shared_ptr<CloudFilterInterface> &filter_ptr,
                        const YAML::Node &config_node);

        bool InitBoxFilter(const YAML::Node &config_node);

        bool SetInitPose(const Eigen::Matrix4f &init_pose);

        bool InitGlobalMap();

        bool ResetLocalMap(float x, float y, float z);

    private:

        pcl::PointCloud<pcl::PointXYZINormal>::Ptr mapGNSSTraj_;

        std::shared_ptr<BoxFilter> box_filter_ptr_;
        std::shared_ptr<CloudFilterInterface> frame_filter_ptr_;
        std::shared_ptr<CloudFilterInterface> local_map_filter_ptr_;
        std::shared_ptr<CloudFilterInterface> global_map_filter_ptr_;
        std::shared_ptr<RegistrationInterface> registration_ptr_;

        CloudData::CLOUD_PTR local_map_ptr_;
        CloudData::CLOUD_PTR global_map_ptr_;
        CloudData::CLOUD_PTR current_scan_ptr_;
        Eigen::Matrix4f current_pose_ = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f init_pose_ = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f current_gnss_pose_ = Eigen::Matrix4f::Identity();
        Eigen::Matrix4f result_pose_ = Eigen::Matrix4f::Identity();

        bool has_inited_ = false;
        bool has_new_global_map_ = false;
        bool has_new_local_map_ = false;

        float prior_ori_ = 0.f;

//        用于点云
        std::string init_key_frames_path_ = "";
        std::vector<Eigen::Matrix4f> init_key_frame_poses_;
//        std::vector<Eigen::Matrix4f,Eigen::aligned_allocator<Eigen::Matrix4f> > init_key_frame_poses_;
        int init_extend_frame_num_ = 3;
        float init_fitness_score_limit_ = 2.0;
        SCManager init_scmanager_;

        YAML::Node config_node_;

        std::shared_ptr<TicToc> time_local;
        std::shared_ptr<TicToc> time_match;
        std::shared_ptr<TicToc> time_filter;

        bool use_wheel;
    };
}

#endif